#include "PID.h"
#include <Arduino.h>

PID::PID()
{
	settings = enabled;
	integrator=0.0;
	lastInput=0.0;
	limitMax=0.0;
	limitMin=0.0;
	pGain=0.0;
	iGain=0.0;
	dGain=0.0;
	lastTime = 0;
}

//getters
double PID::getPGain(){return pGain;}
double PID::getIGain(){return iGain;}
double PID::getDGain(){return dGain;}
double PID::getLimitMax(){return limitMax;}
double PID::getLimitMin(){return limitMin;}
bool PID::isLimitMaxSet(){return settings & allowLimitMax;}
bool PID::isLimitMinSet(){return settings & allowLimitMin;}
bool PID::isInverted(){return settings & invert;}

//setters
void PID::setPGain(double newPGain){pGain=newPGain;}
void PID::setIGain(double newIGain){iGain=newIGain;}
void PID::setDGain(double newDGain){dGain=newDGain;}
void PID::setPIDGain(double newPGain, double newIGain, double newDGain)
{
	setPGain(newPGain);
	setIGain(newIGain);
	setDGain(newDGain);
}
void PID::setLimitMax(double newLimitMax)
{
	limitMax=newLimitMax;
	settings |= allowLimitMax;
}
void PID::setLimitMin(double newLimitMin)
{
	limitMin=newLimitMin;
	settings |= allowLimitMin;
}
void PID::setLimits(double newLimitMin, double newLimitMax)
{
	setLimitMin(newLimitMin);
	setLimitMax(newLimitMax);
}
void PID::disableMaxLimit(){settings &= ~allowLimitMax;}
void PID::disableMinLimit(){settings &= ~allowLimitMin;}
void PID::disableLimits()
{
	disableMaxLimit();
	disableMinLimit();
}
void PID::zeroOutIntegrator(){integrator=0;}
void PID::nullOut()
{
	// empty run
	integrator = 0;
	lastInput = 0;
}
void PID::setInverted(bool newInvert)
{
	if (newInvert){settings |= invert;}
	else {settings &= ~invert;}
}

//actions
void PID::run(const double &input, double &output, const double &setpoint)
{
	if (!settings&enabled){return;}
	long time=millis(),dt;
	dt = time-lastTime;
	if (dt<20){return;}
	if (dt>=1000){lastInput=input;integrator=0;}//time too large for a consistant derivtive (also 0ed out the integrator)
	double error = setpoint-input;
	lastTime=time;
	if (invert){error*=-1.0;}
	integrator+=error*(dt/1000.0);
	//      ,----P-----,  ,-------I-------, ,-----------D----------,
	output=(pGain*error)+(iGain*integrator)+(dGain*(input-lastInput));
	lastInput=input;
	if (isLimitMaxSet() && output>limitMax){output = limitMax;}
	if (isLimitMinSet() && output<limitMin){output = limitMin;}
}

void PID::clear()
{
	pGain=0.0;
	iGain=0.0;
	dGain=0.0;
	settings = enabled;
}

void PID::setFrom(Spid copy)
{
	pGain = copy.p;
	iGain = copy.i;
	dGain = copy.d;
	limitMax = copy.limitMax;
	limitMin = copy.limitMin;
	settings = copy.settings;
}
void PID::copy(Spid &copy)
{
	copy.p = pGain;
	copy.i = iGain;
	copy.d = dGain;
	copy.limitMax = limitMax;
	copy.limitMin = limitMin;
	copy.settings = settings;
}
